#include "balance.h"

#define MOTOR_DIR11 GET_PIN(B, 4)
#define MOTOR_DIR12 GET_PIN(B, 5)
#define MOTOR_DIR21 GET_PIN(B, 6)
#define MOTOR_DIR22 GET_PIN(B, 7)

#define PWM_DEV_NAME "pwm4" /* PWM设备名称 */
#define PWM_LEFT_CHANNEL 3  /* PWM通道 */
#define PWM_RIGHT_CHANNEL 4 /* PWM通道 */
#define PWM_PERIOD 10000000 /* 周期为10ms，单位为纳秒ns */

struct rt_device_pwm *pwm_dev; /* PWM设备句柄 */

#define ENCODER1_DEV_NAME "pulse3" /* PWM设备名称 */
#define ENCODER1_A_PIN GET_PIN(A, 6)
#define ENCODER1_B_PIN GET_PIN(A, 7)
#define ENCODER2_A_PIN GET_PIN(B, 0)
#define ENCODER2_B_PIN GET_PIN(B, 1)

// #define USE_PULSE_ENCODER
rt_device_t encoder_dev = RT_NULL;                  /* 脉冲编码器设备句柄 */
rt_int32_t encoder_left_count, encoder_right_count; //编码器计数输出

static void encoder1_a_irq()
{
    if (rt_pin_read(ENCODER1_B_PIN) == PIN_HIGH)
    {
        encoder_left_count--;
    }
    else
    {
        encoder_left_count++;
    }
}

static void encoder2_a_irq()
{
    if (rt_pin_read(ENCODER2_B_PIN) == PIN_HIGH)
    {
        encoder_right_count++;
    }
    else
    {
        encoder_right_count--;
    }
}

int motor_init(void)
{
    /* 方向引脚 */
    rt_pin_mode(MOTOR_DIR11, PIN_MODE_OUTPUT);
    rt_pin_mode(MOTOR_DIR12, PIN_MODE_OUTPUT);
    rt_pin_mode(MOTOR_DIR21, PIN_MODE_OUTPUT);
    rt_pin_mode(MOTOR_DIR22, PIN_MODE_OUTPUT);

    /* 电机PWM输出 */
    pwm_dev = (struct rt_device_pwm *)rt_device_find(PWM_DEV_NAME);
    if (pwm_dev == RT_NULL)
    {
        rt_kprintf("pwm sample run failed! can't find %s device!\n", PWM_DEV_NAME);
        return RT_ERROR;
    }
    rt_pwm_enable(pwm_dev, PWM_LEFT_CHANNEL);
    rt_pwm_enable(pwm_dev, PWM_RIGHT_CHANNEL);

    /* 电机编码器反馈 */
#if USE_PULSE_ENCODER
    encoder_dev = rt_device_find(ENCODER1_DEV_NAME); //左侧电机编码器
    if (encoder_dev == RT_NULL)
    {
        rt_kprintf("pulse encoder sample run failed! can't find %s device!\n", ENCODER1_DEV_NAME);
        return RT_ERROR;
    }
    if (rt_device_open(encoder_dev, RT_DEVICE_OFLAG_RDONLY) != RT_EOK) //以只读方式打开设备
    {
        rt_kprintf("open %s device failed!\n", ENCODER1_DEV_NAME);
        return RT_ERROR;
    }
#else
    rt_pin_mode(ENCODER1_A_PIN, PIN_MODE_INPUT); //右侧电机编码器
    rt_pin_mode(ENCODER1_B_PIN, PIN_MODE_INPUT);
    rt_pin_attach_irq(ENCODER1_A_PIN, PIN_IRQ_MODE_RISING, encoder1_a_irq, RT_NULL);
    rt_pin_irq_enable(ENCODER1_A_PIN, PIN_IRQ_ENABLE); //使能中断
#endif
    rt_pin_mode(ENCODER2_A_PIN, PIN_MODE_INPUT); //右侧电机编码器
    rt_pin_mode(ENCODER2_B_PIN, PIN_MODE_INPUT);
    rt_pin_attach_irq(ENCODER2_A_PIN, PIN_IRQ_MODE_RISING, encoder2_a_irq, RT_NULL);
    rt_pin_irq_enable(ENCODER2_A_PIN, PIN_IRQ_ENABLE); //使能中断

    return 0;
}
INIT_APP_EXPORT(motor_init);

void set_motor_dir(rt_int32_t index, rt_int32_t dir)
{
    if (index == 0)
    {
        if (dir == 0)
        {
            rt_pin_write(MOTOR_DIR11, PIN_LOW);
            rt_pin_write(MOTOR_DIR12, PIN_HIGH);
        }
        if (dir == 1)
        {
            rt_pin_write(MOTOR_DIR11, PIN_HIGH);
            rt_pin_write(MOTOR_DIR12, PIN_LOW);
        }
    }
    if (index == 1)
    {
        if (dir == 0)
        {
            rt_pin_write(MOTOR_DIR21, PIN_HIGH);
            rt_pin_write(MOTOR_DIR22, PIN_LOW);
        }
        if (dir == 1)
        {
            rt_pin_write(MOTOR_DIR21, PIN_LOW);
            rt_pin_write(MOTOR_DIR22, PIN_HIGH);
        }
    }
}

void set_motor_pwm(rt_int32_t index, rt_int32_t pwm)
{
    // 设置电机方向
    if (pwm > 0)
    {
        set_motor_dir(index, 0);
    }
    else
    {
        pwm = -pwm;
        set_motor_dir(index, 1);
    }

    /* 设置PWM周期和脉冲宽度 */
    if (index == 0)
    {
        rt_pwm_set(pwm_dev, PWM_LEFT_CHANNEL, PWM_PERIOD, pwm);
    }
    if (index == 1)
    {
        rt_pwm_set(pwm_dev, PWM_RIGHT_CHANNEL, PWM_PERIOD, pwm);
    }
}

rt_int32_t get_motor_encoder(rt_int32_t index)
{
    if (index == 0) //左电机
    {
#if USE_PULSE_ENCODER
        rt_device_read(encoder_dev, 0, &encoder_left_count, 1);
        rt_device_control(encoder_dev, PULSE_ENCODER_CMD_CLEAR_COUNT, RT_NULL);
        return encoder_left_count;
#else
        rt_int32_t ret = encoder_left_count;
        encoder_left_count = 0;
        return ret;
#endif
    }
    if (index == 1) //右电机
    {
        rt_int32_t ret = encoder_right_count;
        encoder_right_count = 0;
        return ret;
    }
}
